#include "Envelope.h"
#include <FL/gl.h>

#include <math.h>
#include "global.h"

Envelope::Envelope()
{
    validFrom = 0;
    validTo = 0;
    coeff = 1;
    offset = 0;
    minGrad = gIni.GetDoubleValue("general","SplineMinGradient");

    points = new  multimap<double, double>;
    pointsInterp = new  multimap<double, double>;

    points->insert(Point(0.0 , 08.0));
    points->insert(Point(0.8, 0117));

    points->insert(Point(4.0 , 08.90));
    points->insert(Point(13.0 , 01.90));
    points->insert(Point(15.0 , 12.90));
    points->insert(Point(20.0 , 08.0));
    points->insert(Point(20.8, 0117));

    points->insert(Point(24.0 , 08.90));
    points->insert(Point(33.0 , 01.90));
    points->insert(Point(35.0 , 12.90));
    oldSize = 0;
    setupSpline();
}

Envelope::~Envelope()
{
    delete points;
    delete pointsInterp;
}

void Envelope::draw()
{
    //setupSpline();
    /*
    glBegin(GL_LINE_STRIP);
    //glBegin(GL_POINTS);
    it=pointsInterp->begin();
    for (  ; it!=pointsInterp->end(); it++)
    {
        glVertex2f( it->first, it->second );
    }
    glEnd();
    */
    glBegin(GL_POINTS);
    for (double x=0.0; x<40; x+=0.3)
        glVertex2f( x, getValue(x) );
    glEnd();
}


void Envelope::setupSpline()
{
    float * p = new float[2];
    // the level of detail for the curve
    unsigned int LOD=20;

    multimap<double, double> :: const_iterator itP1 = points->begin();
    multimap<double, double> :: const_iterator itP2 = points->begin();
    multimap<double, double> :: const_iterator itP3 = points->begin();
    multimap<double, double> :: const_iterator itP4 = points->begin();

    for(int start_cv=-3,j=-1;j!=points->size()+2;++j,++start_cv)
	{
		// for each section of curve, draw LOD number of divisions
		for(int i=0;i<=LOD;++i)
		{
			// use the parametric time value 0 to 1 for this curve
			// segment.
			float t = (float)i/LOD;
			// the t value inverted
			float it = 1.0f-t;
			// calculate blending functions for cubic bspline
			float b0 = it*it*it/6.0f;
			float b1 = (3*t*t*t - 6*t*t +4)/6.0f;
			float b2 = (-3*t*t*t +3*t*t + 3*t + 1)/6.0f;
			float b3 =  t*t*t/6.0f;
			// calculate the x,y and z of the curve point

            p[0] = b0 * itP1->first +
					  b1 * itP2->first +
					  b2 * itP3->first +
					  b3 * itP4->first ;
			p[1] = b0 * itP1->second +
					  b1 * itP2->second +
					  b2 * itP3->second +
					  b3 * itP4->second ;

			// specify the point
			pointsInterp->insert(Point(p[0], p[1]));
		}
        if (start_cv + 0 > 0 && start_cv + 0 < points->size()) itP1++;
        if (start_cv + 1 > 0 && start_cv + 1 < points->size()) itP2++;
        if (start_cv + 2 > 0 && start_cv + 2 < points->size()) itP3++;
        if (start_cv + 3 > 0 && start_cv + 3 < points->size()) itP4++;
	}
	thin();
}

void Envelope::thin()
{
    multimap<double, double> :: iterator itP1 = pointsInterp->begin();
    multimap<double, double> :: iterator itP2 = pointsInterp->begin();
    multimap<double, double> :: iterator itP3 = pointsInterp->begin();

    float grad1,grad2,gradDiff;

    itP2++;
    itP3++; itP3++;

    do
    {
        grad1 = (itP2->second - itP1->second) / (itP2->first - itP1->first);
        grad2 = (itP3->second - itP2->second) / (itP3->first - itP2->first);


        gradDiff = grad1 - grad2;

        if (gradDiff < minGrad && gradDiff > -minGrad)
        {
            pointsInterp->erase(itP2);
            itP2=itP3;
            itP3++;        }
        else
        {
            itP1++;
            itP2++;
            itP3++;
        }
    } while (itP3 != pointsInterp->end());
}


double Envelope::getValue(double tSamples)
{
    if (validTo == -1)
        return offset;
    if ( (tSamples <= validTo) && tSamples > validFrom )
        return coeff * tSamples + offset;

    multimap<double, double> :: const_iterator itP2 = pointsInterp->upper_bound( tSamples );
    multimap<double, double> :: const_iterator itP1 = itP2; itP1--;

    if (itP2 == pointsInterp->end())
    {
        validTo == -1;
        offset = itP1->second;
        return offset;
    }

    coeff = (itP2->second - itP1->second) / (itP2->first - itP1->first);
    offset = itP1->second - coeff * itP1->first;

    validFrom = itP1->first;
    validTo   = itP2->first;

    return coeff * tSamples + offset;
};
